May 5th, 2013 MM - forked from "Walking SLIP" notebook
May 16th, 2013 MM - bughunting: comparison between "wrapped" (circular-invariant) and "original" model gives slightly different results - this must not be the case. [edit: solved - that was a tricky one!]
May 31th, 2013 MM - moved to server, added to SVN ... continued bughunting
June 4th, 2013 MM - fixed bug (finally?), found quasiperiodic circular walking solution
June 14th, 2013 MM - searched and analyzed some fixed points
June 27th, 2013 MM - started re-organization (for fixpoint mappings)
July 1st, 2013 MM - cleaned up notebook
July 10th, 2013 MM - continued preparations for mapping; introduced fixed energy solutions
July 11th, 2013 MM - hangling on the edge of stable solutions introduced
July 12th, 2013 MM - continuation for changed alpha introduced
Step 1: initialize notebook
Step 2: find quasi-periodic solutions
2.1 definitions and shortcuts
2.2 compute periodic solution
2.3 visualize result
2.4 verify stability / instability
Step 3: automate fixpoint search
Step 4: continuation for different alpha
Step 5: New approach: "augment" Harti's solutions
General notes
This notebook implements the goals for analyzing the 3D walking gait.
Model parameters have the following structure:
param
.foot1 (1x3 array) location of foot 1
.foot2 (1x3 array) location of foot 2
.m (float) mass
.g (1x3 array) vector of gravity
.lp1 (4x float) list of leg parameters for leg 1
for SLIP: [l0, k, alpha, beta] (may be overwritten in derived models)
.lp2 (4x float) list of leg parameters for leg 2
In [3]:
# Import libraries
from models import bslip
from models.bslip import ICeuklid_to_ICcircle, ICcircle_to_ICeuklid, circ2normal_param, new_stridefunction, vis_sim
from copy import deepcopy # e.g. for jacobian calculation
import mutils.misc as mi
import sys
#define functions
def new_stridefunction_E(pbase, E_des, p_red):
f = new_stridefunction(pbase)
def innerfunction(IC_red):
IC = ICe_to_ICc(IC_red, E_des, p_red[0], pbase['m'], l0 = pbase['lp1'][1])
return f(IC, p_red)[[0,1,3,4]]
return innerfunction
# IC_circle: [y, vy, |v|, |l|, phiv]
def getEnergy(IC_circle, k1, m=80, l0=1):
""" returns the energy of the given state (and with specified params). (the constant term mc**2 is neglected)
:args:
IC_circle: the initial conditions in circular form
k1 : leg stiffness of contact leg
m : mass
l0 : leg rest length
:returns:
E: the total energy of the system
"""
E_kin = m * .5 * IC_circle[2]**2
E_pot = 9.81 * m * IC_circle[0]
E_spring = .5 * k1 * (IC_circle[3] - l0)**2
return E_kin + E_pot + E_spring
def ICe_to_ICc(ICr, E_des, k1, m=80, l0=1):
"""
returns circular ICs with a fixed energy.
:args:
ICr: the reduced circular ICs: [y, vy, |l|, vphi]
E: the desired energy
k1 : leg stiffness of contact leg
m : mass
l0 : leg rest length
:returns:
ICc: (circular) initial conditions
"""
ICc = zeros(5)
ICc[[0,1,3,4]] = ICr
# compute velocity magnitude separately
vmag2 = 2 * (E_des - getEnergy(ICc, k1, m) ) / m
if vmag2 >= 0:
ICc[2] = sqrt(vmag2)
else:
raise ValueError("Velocity magnitude must be imaginary!")
return ICc
def deltafun_E_base(ICe, p_red, p_base):
""" returns the difference of the IC minus the final state """
f = new_stridefunction(p_base)
ICc = ICe_to_ICc(ICe, E_des, p_red[0], p_base['m'], l0 = p_base['lp1'][1])
return array(f(ICc, p_red))[[0,1,3,4]] - array(ICe)
def getPS(ICr, pred, pbase, E_des, maxStep=.1, debug=False, rcond=1e-7, maxnorm=5e-6, maxrep_inner=12,
get_EV = False, h=1e-4):
"""
searches a periodic solution
:args:
ICr [array (4)]: reduced initial conditions to start from: [y, vy, |l|, vphi]
pred: reduced set of parameters
pbase: base set of parameters
:returns:
(stable, ICp): stable is True if the solution is stable, and ICp give the periodic solution
in circular coordinates (5x)
:raises:
RuntimeError: if too many iterations were necessary
"""
# set algorithm parameter
stab_thresh = 1.002 # maximum value for largest EV such that solution is considered stable. # old: for num. accuracy
stab_thresh = 1.00 # maximum value for largest EV such that solution is considered stable.
all_norms = []
deltafun_E = lambda x: deltafun_E_base(x, pred, pbase)
IC_next_E = ICr.copy()
n_bisect_max = 4
nrep = 0
# This is the Newton-Raphson algorithm (except that the inverse is replaced by a pseudo-inverse)
r_norm = norm(deltafun_E(IC_next_E)) #some high value
while r_norm > maxnorm and nrep < maxrep_inner:
J = mi.calcJacobian(deltafun_E, IC_next_E, h=h)
# compute step (i.e. next IC). limit stepsize (only if start is too far away from convergence)
delta0 = - dot(pinv(J, rcond=rcond), deltafun_E(IC_next_E)).squeeze()
if norm(delta0) > maxStep:
delta0 = delta0 / norm(delta0) * maxStep
sys.stdout.write('!')
else:
sys.stdout.write('.')
# update position
IC_next_E = IC_next_E + delta0
nrep += 1
r_norm_old = r_norm
r_norm = norm(deltafun_E(IC_next_E))
all_norms.append(r_norm)
# check if norm decreased - else, do a bisection back to the original point
if r_norm > r_norm_old:
# error: distance INcreased instead of decreased!
new_dsts = []
smallest_idx = 0
maxnorm_bs = r_norm
sys.stdout.write('x(%1.2e)' % r_norm)
for niter_bs in range(5):
IC_next_E = IC_next_E - (.5)**(niter_bs + 1) * delta0
new_dsts.append([IC_next_E.copy(), norm(deltafun_E(IC_next_E))])
if new_dsts[-1][1] < maxnorm_bs:
maxnorm_bs = new_dsts[-1][1]
smallest_idx = niter_bs
IC_next_E = new_dsts[smallest_idx][0]
#while r_norm > r_norm_old:
#
# n_bisect += 1
# IC_next_E = IC_next_E - (.5)**n_bisect * delta0
# r_norm = norm(deltafun_E(IC_next_E))
# sys.stdout.write('x(%1.2e)' % r_norm)
#
# if n_bisect >= n_bisect_max:
# break
if r_norm < maxnorm:
print " success!",
is_stable = True
# compute stability!
# old: take full state into account.
#f = new_stridefunction(pbase)
IC_circle = ICe_to_ICc(IC_next_E, E_des, pred[0], pbase['m'],l0 = pbase['lp1'][1])
#J = mi.calcJacobian(lambda x: f(x, p_red), IC_final)
# new: take only energy-constant values into account
f = new_stridefunction_E(pbase, E_des, pred)
J = mi.calcJacobian(f, IC_next_E)
#print "eigenvalues of Jacobian:"
if max(abs(eig(J)[0])) > stab_thresh:
is_stable = False
if get_EV:
return eig(J)[0], IC_circle
else:
return is_stable, IC_circle
else:
print "number of iterations exceeded - aborting"
#print all_norms
print "IC:", IC_next_E
raise RuntimeError("Too many iterations!")
def getEig(sol):
""" returns the eigenvalues of a pair of [icc, pr] """
icc, pr = sol
f = new_stridefunction_E(pbase, E_des, pr)
J = mi.calcJacobian(f, icc[[0,1,3,4]])
return eig(J)[0]
In [2]:
# start model with an almost circular-periodic, stable solution, and visualize the result
mdl = bslip.BSLIP_newTD(bslip.demo_p, bslip.demo_IC)
for rep in arange(20):
_ = mdl.do_step()
fig = vis_sim(mdl)
2.1 definitions and shortcuts
2.2 compute periodic solution
2.3 visualize result
2.4 verify stability / instability
A quasi-periodic solution is a two-step solution that yields "circular walking".
We look at Take-off as Poincare-Section. A solution is quasi-periodic if at the first and last Poincare-Section
Note in energy-preserving models, conditions 1-4 have one redundant criterion.
Here, for periodic solutions in BSLIP, we need to restrict ourselves to a set of four variable parameters.
In a second step, we need to proof that an (n-4)-dimensional subspace of configurations yields solutions with the same final apex state.
This will be done by modifiying the "fixed" set of parameters and searching for a new periodic solution with the same characteristics.
Note that we can choose to add two additional criteria / constraints to have a set of six uniquely defined parameters, which are (conditions):
In [2]:
# select parameter set
pbase = bslip.demo_p
pbase['delta_beta'] = 0
p_red = bslip.demo_p_reduced
# deltafun: find zeros of this function -> this is a periodic solution!
# note: The jacobian is singular because of the energy neutrality
# (stridefunction has one eigenvalue of 1, in the delta-function this goes to 0)
def deltafun(IC):
f = new_stridefunction(pbase)
return array(f(IC, p_red)) - array(IC)
NOTE you have to use the pseudo-inverse because the mapping contains the tangential direction of the trajectory. That is, there will be a corresponding eigenvalue of 0 which makes the inversion of the matrix impossible.
UPDATE actually, because we are looking for roots of the difference function "step(IC) - IC", this should be the case for the "energy-related" eigenvalue (which is 1).
In [ ]:
In [4]:
maxnorm = 1e-6
delta = 1
maxrep = 20 # maximum number of iterations
#IC_next = array([ 0.94962353, 0.40808309, 1.22425653, 0.96290436, -0.04888218]) # this is almost a periodic solution (instable?)
IC_next = array([ 0.92965291, 0.58244793, 1.33387061, 0.94291404, -0.03292294]) # this is another almost periodic solution
#scale proposed step size
# *NOTE* This is only to avoid divergence if you start far from the fixpoint.
# If you are close, this value *MUST* be 1 in order to converge reasonably fast
stepscale = 1.
nrep = 0
# This is the Newton-Raphson algorithm (except that the inverse is replaced by a pseudo-inverse)
while norm(deltafun(IC_next)) > maxnorm and nrep < maxrep:
J = mi.calcJacobian(deltafun, IC_next)
# compute step (i.e. next IC). limit stepsize (only if start is too far away from convergence)
IC_next = IC_next - stepscale * dot(pinv(J, rcond=1e-4), deltafun(IC_next)).squeeze()
nrep += 1
print "rep.", nrep, ": new IC:", IC_next
r_norm = norm(deltafun(IC_next))
print "norm (delta)", r_norm
print "-" * 20
if r_norm < maxnorm:
print "success!"
# compute stability!
f = new_stridefunction(pbase)
J = mi.calcJacobian(lambda x: f(x, p_red), IC_next)
print "eigenvalues of Jacobian:"
print abs(eig(J)[0])
In [8]:
maxnorm = 1e-6
delta = 1
maxrep = 20 # maximum number of iterations
IC_next_E = array([ 0.92965291, 0.58244793, 0.94291404, -0.03292294]) # velocity magnitude will be computed on the base of a fixed total energy
E_des = 816 # 816 J is what Harti uses in his 2006 Paper: "Compliant leg behavior explains ..."
#scale proposed step size
# *NOTE* This is only to avoid divergence if you start far from the fixpoint.
# If you are close, this value *MUST* be 1 in order to converge reasonably fast
#stepscale = 1.
def new_stridefunction_E(pbase, E_des, p_red):
f = new_stridefunction(pbase)
def innerfunction(IC_red):
IC = ICe_to_ICc(IC_red, E_des, p_red[0], pbase['m'], l0 = pbase['lp1'][1])
return f(IC, p_red)[[0,1,3,4]]
return innerfunction
# IC_circle: [y, vy, |v|, |l|, phiv]
def deltafun_E(IC):
""" returns the difference of the IC minus the final state """
f = new_stridefunction(pbase)
IC = ICe_to_ICc(IC, E_des, p_red[0], pbase['m'], l0 = pbase['lp1'][1])
return array(f(IC, p_red)) - array(IC)
In [ ]:
nrep = 0
# This is the Newton-Raphson algorithm (except that the inverse is replaced by a pseudo-inverse)
while norm(deltafun_E(IC_next_E)) > maxnorm and nrep < maxrep:
J = mi.calcJacobian(deltafun_E, IC_next_E)
# compute step (i.e. next IC). limit stepsize (only if start is too far away from convergence)
IC_next_E = IC_next_E - stepscale * dot(pinv(J, rcond=1e-8), deltafun_E(IC_next_E)).squeeze()
nrep += 1
print "rep.", nrep, ": new IC:", IC_next_E
r_norm = norm(deltafun_E(IC_next_E))
print "norm (delta)", r_norm
print "-" * 20
if r_norm < maxnorm:
print "success!"
# compute stability!
f = new_stridefunction(pbase)
IC_final = ICe_to_ICc(IC_next_E, E_des, p_red[0], pbase['m'],l0 = pbase['lp1'][1])
J = mi.calcJacobian(lambda x: f(x, p_red), IC_final)
print "eigenvalues of (full) Jacobian:"
print abs(eig(J)[0])
f2 = new_stridefunction_E(pbase, E_des, p_red)
JE = mi.calcJacobian(f2, IC_next_E)
print "eigs of (energy preserving) Jacobian:", abs(eig(JE)[0])
print "energy of solution:", getEnergy(IC_final, p_red[0])
else:
print "number of iterations exceeded - aborting"
raise RuntimeError("Too many iterations!")
In [5]:
In [5]:
In [6]:
# convert to euklidean coordinates
ICp = ICcircle_to_ICeuklid(IC_next)
pmdl = bslip.BSLIP_newTD(circ2normal_param(pbase, p_red), ICp)
pmdl.do_step()
pmdl.do_step()
figure()
vis_sim(pmdl)
# compute "difference to periodicity"
snf = pmdl.state.copy()
snf[:3] -= pmdl.params['foot1']
ICfc = ICeuklid_to_ICcircle(snf)
print "difference to 'circular' periodicity:", IC_next - array(ICfc)
There are two interesting eigenvalues: one with magnitude 1, another one with magnitude 0. This is not surprising but expected: One (mag. 1) corresponds to the fact that there is an "adjacent" periodic solution for every energy within a given interval. The other one corresponds to disturbances tangent to the trajectory. These "disturbances" will not alter the intersection point at the next Poincare-section.
In [5]:
# make several steps
dst = array([0.001, .0015, 0, 0, 0, .005]) * 10 # disturbane
ICp = ICcircle_to_ICeuklid(IC_next)
print ICp
#print test_pars
#dstmdl = bslip.BSLIP_newTD(circ2normal_param(p0_sym, test_pars), ICp + dst)
dstmdl = bslip.BSLIP_newTD(circ2normal_param(pbase, p_red), ICp + dst)
for rep in range(24): #26):
_ = dstmdl.do_step()
figure()
f = vis_sim(dstmdl)
f.axes[0].set_ylim(.97,.99)
figure(figsize=(14,6))
for ts,td, fs, fd in zip(dstmdl.t_ss_seq, dstmdl.t_ds_seq, dstmdl.forces_ss_seq, dstmdl.forces_ds_seq):
plot(ts, fs[:,1],'r')
plot(ts, fs[:,4],'r--')
plot(td, fd[:,1],'r')
plot(td, fd[:,4],'r--')
Step 3.1 finding the edge
Step 3.2 hangling along the edge
table of content
requirements for algorithm:
approach (new):
Figure 1:
Figure 2:
Figure 3:
Further required plots:
In [10]:
# select parameter set
p_base = bslip.demo_p
p_base['delta_beta'] = 0
p_red = array(bslip.demo_p_reduced) # k1, k2, alpha(1&2), beta(1&2) [NOTE: beta2 += pbase"delta beta"]
E_des = 816 # 816 J is what Harti uses in his 2006 Paper: "Compliant leg behavior explains ..."
#scale proposed step size
# *NOTE* This is only to avoid divergence if you start far from the fixpoint.
# If you are close, this value *MUST* be 1 in order to converge reasonably fast
#stepscale = 1.
# this function calls the external variables "p_red" and "pbase"
# the roots of this function are periodic initial conditions!
In [573]:
hl = mio.mload('h_solutions_a67.0.list')
hl[-1]
Out[573]:
In [606]:
# stage 1: find border! (assuming that initial FP is stable -> to check!)
pbase = bslip.demo_p
pbase['delta_beta'] = 0
p_red = bslip.demo_p_reduced
p_red = array(p_red)
p_red[2] = 63 * pi / 180. # check these from harti
p_red[0] = 9000.
p_red[1] = 9000.
ICe = array([ 0.92965291, 0.58244793, 0.94291404, -0.03292294]) # reasonable starting point: alpha=67
ICe = array([ 0.91046189, 0.54707628, 0.93084559, -0.15405735])
In [607]:
p_solutions = [] # list of (pred, IC) which are 'circular' periodic solutions
is_stable = True
step_k_0 = 400.
min_step_k = 50.
step_k = step_k_0
while step_k > min_step_k:
print "k1:", p_red[0], " ",
is_stable, IC_final = getPS(ICe, p_red, pbase, E_des)
if not is_stable:
print " unstable"
p_red[0] -= step_k
step_k = step_k / 2.
print " reducing stepsize to ", step_k
else:
print " stable"
p_solutions.append((p_red.copy(), IC_final.copy()))
# update next starting point
ICe = IC_final[[0,1,3,4]]
#modify params!
p_red[0] += step_k
In [608]:
p_red[2] * 180 / pi
Out[608]:
In [609]:
# calculate stability:
pr, IC = p_solutions[-1]
ICr = IC[[0,1,3,4]]
f = new_stridefunction_E(pbase, E_des, pr)
J = mi.calcJacobian(f, ICr)
print "eigenvalues of J:", abs(eig(J)[0])
In [610]:
## NOW: found starting point ("edge")!
h_solutions = []
h_solutions.append(p_solutions[-1])
pr0, IC = p_solutions[-1]
ICe = IC[[0,1,3,4]]
E_des = 816 # 816 J is what Harti uses in his 2006 Paper: "Compliant leg behavior explains ..."
pr_new = pr0.copy()
facs_orig = ( [-1,1], [1, 1], [1, -1], [-1, -1] )
facs = [x for x in facs_orig]
last_nrep = 2
In [691]:
#[abs(getEig(x[1:])) for x in adj_solutions]
#abs(getEig(h_solutions[-1][::-1]))
#h_solutions = h_solutions[:-1]
In [657]:
step_k_border = 30.
niter = 60
#pr_new[:2] = array([11000, 6000])
# stage 1: compute up to four points
for borderpoint_nr in range(niter):
print "=" * 20, " #", borderpoint_nr, "=" * 20
fac_idx = roll(arange(4), - (last_nrep - 2))
facs = [facs[idx] for idx in fac_idx]
adj_solutions = []
edge_found = False
rep = 0
edge = None # pair of fixpoints: (pred, ICc), (pred, ICc) [unstable | stable]
while edge == None and rep < 4:
pr_d = pr_new.copy()
pr_d[0] += facs[rep][0] * step_k_border
pr_d[1] += facs[rep][1] * step_k_border
is_stable, ICp = getPS(ICe, pr_d, pbase, E_des)
adj_solutions.append( (is_stable, ICp.copy(), pr_d.copy()) )
rep += 1
# can we break?
if len(adj_solutions) > 1:
if adj_solutions[-2][0] == False and adj_solutions[-1][0] == True:
edge = [adj_solutions[-2][1:], adj_solutions[-1][1:]]
# potentially the correct edge is "last -> first"?
if rep==4 and edge is None:
rep = 5
if adj_solutions[-1][0] == False and adj_solutions[0][0] == True:
edge = [adj_solutions[-1][1:], adj_solutions[0][1:]]
else:
raise RuntimeError("no valid edge detected ... very strange ...")
# guessed solution: weighted average accoring to eigenvalue magnitudes
w1 = max(abs(getEig(edge[0]))) - 1
w2 = max(abs(getEig(edge[1]))) - 1
fac = - w1 / (w2 - w1)
if fac < 0:
print "warning - factor < 0 !!!"
fac = 0
if fac > 1:
print "warning - factor > 1 !!!"
fac = 1
# guess new point on the edge
new_sol_interp = [edge[0][0] + fac * (edge[1][0] - edge[0][0]),
edge[0][1] + fac * (edge[1][1] - edge[0][1])]
# find periodic solution of that point
icc, pr_new = new_sol_interp
ICe = icc[[0,1,3,4]]
# get periodic ICs to new point
is_stable, ICp = getPS(ICe, pr_new, pbase, E_des)
ICe = ICp[[0,1,3,4]]
h_solutions.append( (array(pr_new), array(ICp) ))
last_nrep = rep
print "\n",
In [536]:
ICe = h_solutions[-1][1][[0,1,3,4]]
print ICe
In [673]:
print pr_new[2] * 180 / pi
pr_new2 = pr_new.copy()
pr_new2[0] = 10800
pr_new2[1] = 6200
is_stable, ICp_t = getPS(ICe, pr_new2, pbase, E_des, maxStep=.1, rcond=1e-2)
In [668]:
ICe_2 = ICe_to_ICc(ICe, E_des, pr_new2[0])
vals = getEig([ICe_2, pr_new2])
print abs(vals)
In [603]:
### store data from time to time
import mutils.io as mio
fname = 'h_solutions_a%2.1f.list' % (pr0[2] * 180 / pi)
mio.msave(fname, h_solutions)
print "saved as ", fname
In [637]:
all_k1 = [x[0][0] for x in h_solutions]
all_k2 = [x[0][1] for x in h_solutions]
figure(figsize=(16,9))
plot(all_k1, all_k2, 'rd--')
plot(all_k1[-1], all_k2[-1], 'bd')
axis('equal')
Out[637]:
In [644]:
abs(getEig(h_solutions[-1][::-1]))
Out[644]:
In [60]:
is_stable
#ICc = ICe_to_ICc(ICp, E_des, pr_d[0])
pr_d, ICp = p_solutions[-1]
f = new_stridefunction(pbase)
res = f(ICp, pr_d)
print res - ICp
In [39]:
#print ICp
# adj_solutions
def printPer(sol):
_, icc, pr = sol
ice = icc[[0,1,3,4]]
f = new_stridefunction_E(pbase, E_des, pr)
print f(ice) - ice
def printEig(sol):
_, icc, pr = sol
f = new_stridefunction_E(pbase, E_des, pr)
J = mi.calcJacobian(f, icc[[0,1,3,4]])
print abs(eig(J)[0])
In [58]:
mdl_v.ode.ODE_RTOL
Out[58]:
In [40]:
# select a solution
#pr, IC = p_solutios[1]
#pr_v, IC_v = h_solutions[-1]
#pr_v = pr_new2
#IC_v = ICe_to_ICc([ 0.87159907, 0.71271936, 0.89772377, -0.22796864], E_des, pr_v[0])
#IC_v = ICe_to_ICc([ 0.87116539, 0.63895586, 0.88974478, -0.19038532], E_des, pr_v[0])
p_red[0] = p_red[1] = 14000.
p_red[2] = 73. * pi / 180.
p_red[3] = 0.00
#IC0 = array([ 0.96972, 0.254, 1.15633538, 0.972, 0])
#IC0 = array([ 0.97, 0.2556976, 1.15633538, 0.9721, 0])
#IC0 = array([ 0.974, 0.1556976, 1.15633538, 0.9751, 0])
IC0 = array([0.9278323130603543, 0.34177055981459481, 1.1785275470101138, 0.93209238227003965, 0.0]) # for "B" periodic, very very marginally stable
#IC0 = array([0.97236992204999806, 0.18072413876424653, 1.0718924702189911, 0.97368293371122483, 0.0]) # for "C" with alpha = 76.5
pr_v = p_red
#pr_v[3] = .05
#IC_v = IC0.copy()
ICx = IC0.copy()
#ICx[0] -= .001
IC_v = ICe_to_ICc(ICx[[0,1,3,4]], E_des, pr_v[0])
print IC_v
pbase = p_base
#pr, IC = r_solutions[44] # -7 and 44 are approximately the "symmetric" solutions; 10 and -25 are approximately opposite "9600 / 14500"
#pr, IC = new_a_solutions[-1] # check the last solution if you can see why it failed
# create model
ICe_v = ICcircle_to_ICeuklid(IC_v)
mdl_v = bslip.BSLIP_newTD(circ2normal_param(pbase, pr_v), ICe_v)
mdl_v.ode.ODE_ATOL = 1e-11
mdl_v.ode.ODE_RTOL = 1e-12
mdl_v.ode.ODE_EVTTOL = 1e-12
# make first two steps for calculating walking radius
for rep in range(2):
_ = mdl_v.do_step()
l_v = norm(mdl_v.state[:3] - ICe_v[:3]) # this works *only* because the height is equal!
phi0_v = arctan2(ICe_v[5], ICe_v[3])
phiE_v = arctan2(mdl_v.state[5], mdl_v.state[3])
deltaPhi_v = phiE_v - phi0_v
if abs(deltaPhi_v) < 1e-4:
print "walking radius very large (probably > 10km)"
else:
print "walking radius [m]: ", l_v / (2. * sin(.5 * deltaPhi_v))
for rep in range(20):
try:
_ = mdl_v.do_step()
except bslip.SimulationError:
pass
#figure()
f_v = vis_sim(mdl_v)
#f_v.axes[0].set_ylim(.96,.98)
figure(figsize=(14,6))
for ts_v,td_v, fs_v, fd_v in zip(mdl_v.t_ss_seq, mdl_v.t_ds_seq, mdl_v.forces_ss_seq, mdl_v.forces_ds_seq):
plot(ts_v, fs_v[:,1],'r')
plot(ts_v, fs_v[:,4],'r--')
plot(td_v, fd_v[:,1],'r')
plot(td_v, fd_v[:,4],'r--')
In [38]:
tmp = mdl_v.state.copy()
tmp[:3] -=mdl_v.params['foot1']
print ICeuklid_to_ICcircle(tmp)
IC0 = ICeuklid_to_ICcircle(tmp)
In [54]:
state_e = mdl_v.state.copy()
state_e[:3] -= mdl_v.params['foot1']
print getEnergy(ICeuklid_to_ICcircle(state_e), p_red[0])
ICc = array(ICeuklid_to_ICcircle(state_e))
print "periodicity:", deltafun_E_base(ICc[[0,1,3,4]], p_red, p_base)
print "IC (circular):", ICc
In [28]:
print ICc
In [21]:
for rep in range(20):
_ = mdl_v.do_step()
In [ ]:
f = bslip.vis_sim(mdl_v)
#f.axes[0].set_xlim(8.5, 9.5)
ICx = mdl_v.state.copy()
ICx[:3] -= mdl_v.params['foot1']
print ICeuklid_to_ICcircle(ICx)
print ICx
#ICeuklid_to_ICcircle(
#ICeuklid_to_ICcircle(
In [78]:
mdl_v.params
Out[78]:
In [190]:
import mutils.io as mio
## regularize h_solutions
## h-solutions (and r_solutions) are a list, with elements of this format: [pred, icc]
h_solutions[-1]
r_solutions = [h_solutions[1], ]
min_dk = 190. # minimal distance of k's
for sol in h_solutions[1:]:
# is sol close to any parameter set already in r_solutions?
is_close = False
for rsol in r_solutions:
if norm(rsol[0][:2] - sol[0][:2]) < min_dk:
is_close = True
break
if not is_close:
r_solutions.append(sol)
#r_solutions[0], r_solutions[1] = r_solutions[1], r_solutions[0]
r_solutions = r_solutions[:-2]
mio.msave('r_solutions_68.5.list', r_solutions)
In [518]:
all_k1 = [x[0][0] for x in r_solutions]
all_k2 = [x[0][1] for x in r_solutions]
all_kr1 = [x[0][0] for x in new_a_solutions]
all_kr2 = [x[0][1] for x in new_a_solutions]
figure(figsize=(10,9))
plot(all_k1, all_k2, 'rd-', label='alpha = 67.8')
plot(all_kr1, all_kr2, 'bd-', label='alpha = 67.6')
plot(all_k1[0], all_k2[0], 'go')
plot(all_kr1[5], all_kr2[5], 'mo',)
axis('equal')
title('r_solutions (h_solutions, regularized distances ...')
legend()
Out[518]:
In [514]:
steplength_k_da = 180. # change k by this amount perpendicular to circle!
new_alpha = 65.6 * pi / 180
new_a_solutions = []
sel_idx = 0
In [515]:
# insert last value to position #swapidx and remove last element
#swapidx = 5
#old_val = new_a_solutions[swapidx][:]
#new_a_solutions[swapidx] = new_a_solutions[-1]
#new_a_solutions = new_a_solutions[:-1]
In [520]:
steplength_k_da = 130. # 135. # change this if solution finding fails ...
max_sel_idx = len(r_solutions) # adapt this if you only want to go some steps ...
#max_sel_idx = 87 # adapt this if you only want to go some steps ...
# re-compute only a single value
sel_idx = 93
max_sel_idx = sel_idx + 1
print "current idx: ", sel_idx
In [522]:
#while sel_idx < len(r_solutions):
# pass
while sel_idx < max_sel_idx:
print "=" * 20, " #", sel_idx + 1 , "/", len(r_solutions), "=" * 20
pr_c = r_solutions[sel_idx][0]
pr_new = pr_c.copy()
ICc = r_solutions[sel_idx][1].copy()
# interpolate between two values
#ICc = .5 * (new_a_solutions[sel_idx - 1][1] + new_a_solutions[sel_idx + 1][1].copy())
#pr_new[:2] = .5 * (new_a_solutions[sel_idx - 1][0][:2] + new_a_solutions[sel_idx + 1][0][:2])
pr_new[2] = new_alpha
#get solution for slightly changed alpha
#is_stable, ICp = getPS(ICpx2[[0,1,3,4]], pr_new, pbase, E_des)
is_stable, ICp = getPS(ICc[[0,1,3,4]], pr_new, pbase, E_des)
# search direction in k1-k2-plane:
kA1, kA2 = r_solutions[sel_idx][0][:2]
kB1, kB2 = r_solutions[sel_idx -1][0][:2]
dk1 = kA1 - kB1
dk2 = kA2 - kB2
ldk = sqrt(dk1**2 + dk2**2)
dk1 = dk1 / ldk * steplength_k_da
dk2 = dk2 / ldk * steplength_k_da
# adapt new position "perpendicular" to circle
pr_newx = pr_new.copy()
if is_stable: # new solution is stable? -> go out of circle
pr_newx[0] -= dk2
pr_newx[1] += dk1
else:
pr_newx[0] += dk2
pr_newx[1] -= dk1
# get periodic solution for new parameters
_, ICpx = getPS(ICp[[0,1,3,4]], pr_newx, pbase, E_des)
# get eigenvalues of jacobian of these solutions
w1_all = abs(getEig([ICp, pr_new] ))
w2_all = abs(getEig([ICpx, pr_newx] ))
w1 = max(w1_all) - 1
w2 = max(w2_all) - 1
fac = - w1 / (w2 - w1)
# limit factors and give warnings if extrapolation > 50% interval size
if fac < -.75:
print "warning - factor < -.75 !!! (", fac, ")"
fac = -.75
if fac > 1.75:
print "warning - factor > 1.75 !!! (", fac, ")"
fac = 1.75
# guess new point on the edge where max(abs(eig)) == 1.0
pr_intp = pr_new + fac * (pr_newx - pr_new)
IC_intp = ICp + fac * (ICpx - ICp)
_, ICpx2 = getPS(IC_intp[[0,1,3,4]], pr_intp, pbase, E_des)
wx_all = abs(getEig([ICpx2, pr_intp] ))
print "eigenvalues of new solution (mag.):", wx_all
new_a_solutions.append([pr_intp, ICpx2])
sel_idx += 1
In [302]:
print w0, w1, w2, max(wx_all) - 1
w1_all
w2_all
print fac
print pr_newx - pr_intp
print ICpx - IC_intp
print "eigs:", abs(getEig(r_solutions[45][::-1]))
In [299]:
Out[299]:
In [512]:
fname = 'r_solutions_%2.1f.list' % (new_alpha * 180. / pi)
mio.msave(fname, new_a_solutions)
print "saved new_a_solutions as ", fname
In [513]:
# prepare for new round :)
r_solutions = new_a_solutions
In [116]:
figure()
plot(kA1, kA2, 'rd')
plot(kB1, kB2, 'gd')
axis('equal')
dk1 = kA1 - kB1
dk2 = kA2 - kB2
new_k = [dk2, -dk1]
plot(kA1 + new_k[0], kA2 + new_k[1], 'bd')
plot(pr_newx[0], pr_newx[1], 'm+')
Out[116]:
In [65]:
p_red = array(bslip.demo_p_reduced)
E_des = 816.
p_base = bslip.demo_p
p_base['delta_beta'] = 0
selection = 3
# periodic solution 1:
if selection == 1:
p_red[0] = p_red[1] = 14000
p_red[2] = 69. * pi / 180.
p_red[3] = .05
## ?? IC0 = array([ 0.93358044, 0.43799566, 1.25842366, 0.94657333, -0.10969046]) # already periodic (?)
IC0 = array([ 0.93358034, 0.43799844, 1.25842356, 0.94657321, 0.10969058]) # beta=.05
#IC0 = array([ 0.93358039, 0.45195548, 1.26003517, 0.94679076, 0.21853576]) # beta=.1
elif selection == 2:
# periodic solution 2: (asymmetric stance phase)
p_red[0] = p_red[1] = 14000
p_red[2] = 72.5 * pi / 180.
p_red[3] = 0.05
#IC0 = array([ 0.92172543, 0.40671347 , 1.1950172, 0.92609043 , 0. ]) # periodic for beta = 0, alpha=72.5
IC0 = array([ 0.9308592, 0.3793116, 1.19155584, 0.9360028, 0.1597469 ]) # for beta=.05, alpha=72.5
#IC0 = array([ 0.93011364, 0.39346135, 1.19332777, 0.93554008, 0.31541887]) # for beta=.1, alpha=72.5
# periodic, for beta=0, alpha=73. very very marginally stable: |ev| = .992|.984 (step|stride):
#IC0 = array([ 0.9278273, 0.34175418, 1.17852052 , 0.93208755 , 0.])
elif selection == 3:
# periodic solution 3:
p_red[0] = p_red[1] = 20000
p_red[2] = 76.5 * pi / 180.
p_red[3] = 0.1
#p_red[3] = 0.0
#IC0 = array([ 0.96030477, 0.30256976, 1.15633538, 0.985058303, -0.11240564])
#IC0 = array([ 0.97043906, 0.29739433, 1.0840199, 0.97280541, 0. ]) # for beta=0; not really periodic (2e-4)
IC0 = array([ 0.97236992, 0.18072418, 1.0718928, 0.97368293, 0. ]) # for beta=0, alpha=76.5
IC0 = array([ 0.97236992, 0.17616847, 1.07200705, 0.97370148, 0.22353624]) # for beta=.5, alpha=76.5
IC0 = array([ 0.97237007, 0.16336241, 1.07238146, 0.97376284, 0.44756444]) # for beta=.5, alpha=76.5
#IC0 = array([0.9709, 0.34167, 1.0855, 0.973732, 0.15846])
#IC0 = array([ 0.97028136, 0.30045474, 1.08604313, 0.97290092, 0.16489379])
#ICe = IC0[[0,1,3,4]]
#[ 0.97029372 0.2972158 0.97289854 0.16536238]
#ICe = array([ 0.97050506, 0.30422253, 0.97301987, 0.06965177])
##print ICe_to_ICc(ICe, E_des, p_red[0])
##stop
else:
raise NotImplementedError("No valid selection - select solution 1,2, or 3")
#ICe[1] -= .15
#ICe = ICe_to_ICc(ICe, E_des, p_red[0])
#stop
#IC0 = array(ICeuklid_to_ICcircle(bslip.demo_IC))
#print IC0[[0,1,3,4]]
#print ICe
#stop
#ICe_to_ICc(ICe, E_des, p_red[0])
#ICe = array([ 0.95630459 , 0.57246797 , 0.96124696, -0.1443469 ])
ICe = IC0[[0,1,3,4]]
evs, IC = getPS(ICe, p_red, p_base, E_des, get_EV = True, maxnorm=1e-5, rcond=1e-7, maxStep=.1, maxrep_inner=10, h=1e-4)
print IC
print "max. ev:", max(abs(evs))
In [63]:
p_base
Out[63]:
In [48]:
sqrt(.98408)
Out[48]:
In [255]:
print evs
In [37]:
IC0 = ICe_to_ICc( ICe, E_des, p_red[0])
print IC0
print p_red[0], p_red[2] * 180 / pi, p_red[3]
print p_base
print p_red
In [62]:
IC0 = IC
#print abs(evs)
#ICe = array([ 0.97028136 , 0.30045474 , 0.97290092 , 0.16489379])
#IC[ 0.97043906 0.29739433 1.0840199 0.97280541 0. ]
#ICe = array(ICeuklid_to_ICcircle(state_e))[[0,1,3,4]] # from simulation
ICe = array(IC0)[[0,1,3,4]]
#ICe = array([ 0.96972, 0.254, 0.972, 0])
print "periodicity:", deltafun_E_base(ICe, p_red, p_base)
the radius of a quasi-periodic solution can be computed as
$r = \frac{\large l}{\large 2 \sin{(\Delta \varphi / 2)}}$,
where $l$ is the absolute distance between CoM at start and end of a stride, and $\Delta \varphi$ is the angle between initial and final CoM velocity in horizontal plane.
Here, we are mostly interested in "functional" asymmetries of the leg - asymmetries that are not apparent like different leg lengths. These can be differences in the leg strength, here represented by different leg stiffnesses $k$, or differences in the leg swing policies, here represented by different angles of attack $\alpha$.
For completion, we also demonstrate that the "apparent" asymmetries like in leg length or lateral leg placement also yield circular walking behavior.
We investigated three different kind of periodic motions, as already found by Geyer: two different kind of symmetric motions, and an asymmetric motion. We selected three parameter sets similar to Geyer's A,B,C points, but changed them slightly towards configurations with increased stability.